/**
 * @brief 			Braitenberg.cpp Member Function Implementations
 */

#include "Braitenberg.h"
#include <iostream>

Braitenberg::Braitenberg()
{
  std_wr[RIGHT_FRONT].setV( -0.9 );
  std_wr[RIGHT_MIDDLE].setV( 0.8 );
  std_wr[RIGHT].setV( 0.1 );
  std_wr[RIGHT_BACK].setV( 0 );
  std_wr[LEFT_BACK].setV( 0 );
  std_wr[LEFT].setV( -0.1 );
  std_wr[LEFT_MIDDLE].setV( -0.8 );
  std_wr[LEFT_FRONT].setV( 0.987244 );

  std_wl[RIGHT_FRONT].setV( -1.1 );
  std_wl[RIGHT_MIDDLE].setV( -0.8 );
  std_wl[RIGHT].setV( -0.1 );
  std_wl[RIGHT_BACK].setV( 0 );
  std_wl[LEFT_BACK].setV( 0 );
  std_wl[LEFT].setV( 0.1 );
  std_wl[LEFT_MIDDLE].setV( 0.8 );
  std_wl[LEFT_FRONT].setV( -1.7438 );


  for(int i = 0; i < NUM_SENS; ++i)
  {
    lweight[i] = std_wl[i];
    rweight[i] = std_wr[i];
    counter[i] = 0u;
  }
}

Braitenberg::~Braitenberg()
{
  std::cout << "\n//final weights right:\n";
  for(int i = 0; i < NUM_SENS; ++i)
    std::cout << "std_wr[" << printSensor(static_cast<SensorIndex>(i)) << "].setV( " << rweight[i].getV() << " ); \n";

  std::cout << "\n//final weights left:\n";
  for(int i = 0; i < NUM_SENS; ++i)
    std::cout << "std_wl[" << printSensor(static_cast<SensorIndex>(i)) << "].setV( " << lweight[i].getV() << " ); \n";
}

void Braitenberg::input(const Sensors& sensor)
{
  tmp_sensor = sensor;

  for (int i=0; i<NUM_SENS; i++)
  {
    ++counter[i];
    if(sensor[i] < 50)
      tmp_sensor[i] = 50;
  }

  double left, right;

  for(int i = 0; i < NUM_SENS; ++i)
  {
    if(tmp_sensor[i] > SENSOR_MAX)
    {
      lweight[i].randomV();
      rweight[i].randomV();

    }
    if(counter[i] > COUNTER_THRES)
    {
      lweight[i] = std_wl[i];
      rweight[i] = std_wr[i];
      counter[i] = 0u;
    }

    left += tmp_sensor[i] * lweight[i].getV();
    right += tmp_sensor[i] * rweight[i].getV();
  }

  double sum_wl = 0, sum_wr = 0;
  for(int i = 0; i < NUM_SENS; ++i)
  {
    sum_wl += lweight[i].getV();
    sum_wr += rweight[i].getV();
  }

//  if( ! (sum_wl < 0 && sum_wr < 0 ) || (sum_wl > 0 && sum_wl > 0) )
//  {
//    std::cout << "-.---------------------reset\n";
//    for(int i = 0; i < NUM_SENS; ++i)
//    {
//      lweight[i] = std_wl[i];
//      rweight[i] = std_wr[i];
//    }
//  }

  std::cout << "left: " << left << " right: " << right << "\n";
  motor.move(OFFSET + left, OFFSET + right);
}



Motor& Braitenberg::output()
{
  return motor;
}






//
//const unsigned int wait_time  = 10000;
//
//const int basespeed = 10;
//
//const int offset = 500;
//
//const int crash_threshold = 2000; // recognized as "crash"
//
//
//int usage()
//{
//  cout << "./test <robot number>\n\n";
//  cout << "The robot's number can be found on the package.\n";
//  cout << "Use 0 for a dummy interface.\n\n";
//  return 1;
//}
//
//void printIntArray(const string& header, int* array, int size)
//{
//  cout << header;
//  for (int i = 0; i < size; i++)
//    cout << array[i] << " ";
//  cout << endl;
//}
//
//
//void printBoolArray(const string& header, bool* array, int size)
//{
//  cout << header;
//  for (int i = 0; i < size; i++)
//    cout << array[i] << " ";
//  cout << endl;
//}
//
//bool isCrash(int value)
//{
//  if(value > crash_threshold) {
//    return true;
//  }
//  return false;
//}
//
//
//int program( gui::EpuckInterface *epuck )
//{
//  using gui::Toggle;
//  using gui::Int2;
//  using gui::Int3;
//  using gui::Int8;
//
//
//  // variables
//  Int8 prox;
//  Int2 speed;
//
//  bool crashes[8];
//
//
//
//  // front middle right
//  unsigned int coeff_left_prox0 =  1;
//  unsigned int coeff_right_prox0  =  1;
//
//  // front right
//  unsigned int coeff_left_prox1 =  1;
//  unsigned int coeff_right_prox1  =  1;
//
//  // right
//  unsigned int coeff_left_prox2 =  1;
//  unsigned int coeff_right_prox2  =  1;
//
//  // back right
//  unsigned int coeff_left_prox3 =  1;
//  unsigned int coeff_right_prox3  =  1;
//
//  // back left
//  unsigned int coeff_left_prox4 =  1;
//  unsigned int coeff_right_prox4  =  1;
//
//  // left
//  unsigned int coeff_left_prox5 =  1;
//  unsigned int coeff_right_prox5  =  1;
//
//  // front left
//  unsigned int coeff_left_prox6 =  1;
//  unsigned int coeff_right_prox6  =  1;
//
//  // front middle left
//  unsigned int coeff_left_prox7 =  1;
//  unsigned int coeff_right_prox7  =  1;
//
//
//  int speed_left    = 1;
//  int speed_right   = 1;
//
//
//  // initially stop robot
//  epuck->stopRobot();
//
//
//  // calibrate IR sensors (NO OBJECTS MUST BE AROUND!!)
//  cout << "Calibrating proximity sensors...";
//  cout.flush();
//  epuck->calibrateProximitySensors();
//  cout << "done." << endl;
//
//
//  // main loop
//  while(1)
//  {
//
//    // read IR sensor data
//    prox = epuck->getProximitySensors();
//    cout << "done." << endl;
//    printIntArray("Proximity sensor: ", prox.data, 8);
//    /*
//     * prox.data[0] -> front middle right
//     * prox.data[1] -> front right
//     * prox.data[2] -> right
//     * prox.data[3] -> back right
//     * prox.data[4] -> back left
//     * prox.data[5] -> left
//     * prox.data[6] -> front left
//     * prox.data[7] -> front middle left
//     */
//
//
//    // normalize sensor data
//    for (int i=0; i<8; i++)
//    {
//      if(prox.data[i] < 100){
//        prox.data[i] = 100;
//      }
//      prox.data[i] = prox.data[i] / 10;
//    }
//
//
//    // detect crash (check all 8 sensors)
//    for (int i=0; i<8; i++)
//    {
//      crashes[i] = false; // reset
//      crashes[i] = isCrash(prox.data[i]);
//    }
//    /*
//     * crashes[0] -> front middle right
//     * crashes[1] -> front right
//     * crashes[2] -> right
//     * crashes[3] -> back right
//     * crashes[4] -> back left
//     * crashes[5] -> left
//     * crashes[6] -> front left
//     * crashes[7] -> front middle left
//     */
//    printBoolArray("Crashes: ", crashes, 8);
//
//
//
//    // apply coefficients
//    speed_left =  (-coeff_left_prox0 * prox.data[0]) +
//
//            (-coeff_left_prox1 * prox.data[1]) +
//            (coeff_left_prox1 * prox.data[1]) / 2 +
//
//            (-coeff_left_prox2 * prox.data[2]) +
//            (coeff_left_prox3 * prox.data[3]) +
//            (coeff_left_prox4 * prox.data[4]) +
//            (coeff_left_prox5 * prox.data[5]) +
//
//            (coeff_left_prox6 * prox.data[6]) -
//            (coeff_left_prox6 * prox.data[6]) / 2 +
//
//            (-coeff_left_prox7 * prox.data[7]);
//
//    speed_right = (-coeff_right_prox0 * prox.data[0]) +
//
//            (coeff_right_prox1 * prox.data[1]) -
//            (coeff_right_prox1 * prox.data[1]) / 2 +
//
//            (coeff_right_prox2 * prox.data[2]) +
//            (coeff_right_prox3 * prox.data[3]) +
//            (coeff_right_prox4 * prox.data[4]) +
//            (-coeff_right_prox5 * prox.data[5]) +
//
//            (-coeff_right_prox6 * prox.data[6]) +
//            (coeff_right_prox6 * prox.data[6]) / 2 +
//
//
//            (-coeff_right_prox7 * prox.data[7]);
//    /*
//     * prox.data[0] -> front middle right
//     * prox.data[1] -> front right
//     * prox.data[2] -> right
//     * prox.data[3] -> back right
//     * prox.data[4] -> back left
//     * prox.data[5] -> left
//     * prox.data[6] -> front left
//     * prox.data[7] -> front middle left
//     */
//
//
//
//    // set motor speeds
//    epuck->setMotorSpeed(offset + (speed_left * basespeed), offset + (speed_right * basespeed));
//    speed = epuck->getMotorSpeed();
//    printIntArray("Speed: ", speed.data, 2);;
//
//
//    // wait (in microseconds)
//    usleep(wait_time);
//  }
//
//  return 0;
//}
